
#include <OpenNI.h>
#include <GL/glut.h>
#include <Screen.h>
#include <ColorScreen.h>
#include <math.h>


Screen dScreen;
ColorScreen cScreen;

openni::RGB888Pixel*	screen;
int * fscreen;
openni::VideoFrameRef frame;
openni::VideoFrameRef frameColor;

int sdf[640][640][1000];
int result[640][640];


int depthw = 640;
int depthh = 480;

//int sdf[640][640];

int mouse[2];
float cameraX = 0.0f;
float cameraY = 0.0f;
float cameraZ = 0.0f;

// DATA ACESSING FUNCTIONS
#define D(i,j) ((float)-fscreen[i+j*depthw])
#define C(i,j) (screen[i+j*depthw])

// VARIABLES
float cval =18000;
float zstrength =1; // normal z strength
int maxDistance= 2500;
int offset_ = 3;
#define MAX_DISTANCE maxDistance // max viewing distance
#define OFFSET offset_ // offset in point sampling
#define DISTANCE 10 // distance in camera movement
float lightPosition[4] = { -10000,-10000,-100, 1 };
#define CVALUE cval
// CONSTANTS
#define X_DIRECTION 1 
#define Y_DIRECTION 2
#define X 0
#define Y 1
#define Z 2
#define W 3
#define ZSTRENGTH zstrength

// function that updates the sensor data
void draw()
{
	
	if(dScreen.getImage(&frame) && cScreen.getImage(&frameColor))
	{
		openni::DepthPixel* pDepthRow = (openni::DepthPixel*)frame.getData();
		openni::RGB888Pixel* pColorRow = (openni::RGB888Pixel*)frameColor.getData();
		int rowSize = frame.getStrideInBytes() / sizeof(openni::DepthPixel);
		int rowSizeColor = frameColor.getStrideInBytes() / sizeof(openni::RGB888Pixel);

		openni::RGB888Pixel* pScreen = screen;
		int * pfscreen = fscreen;
		for (int y = 0; y < frame.getHeight(); ++y)
		{
			const openni::RGB888Pixel* pColor = pColorRow;
			const openni::DepthPixel* pDepth = pDepthRow;

			for (int x = 0; x < frame.getWidth(); ++x, ++pDepth , ++pColor, ++pScreen, ++pfscreen)
			{
				*pfscreen = (int)*pDepth;
				*pScreen = *pColor;
			}

			pDepthRow += rowSize;
			pColorRow += rowSizeColor;
		}
	}

}

// function to acess the color sensor data
openni::RGB888Pixel getColor(int i, int j)
{
	if(i<depthw && i >=0 && j<depthh && j>=0)
	{
		return C(i,j);
	}
	else
	{
		return C(0,0);
	}
}

// gets the depth and interpolate if it is a floating number
float getDepth(float i, float j)
{
	float di,fi;
	float dj,fj;
	float daux1, daux2;
	if(i<depthw && i >=0 && j<depthh && j>=0)
	{
		di = floor(i);
		fi = i-di;
		dj = floor(j);
		fj = j-dj;
		if(fi>0)
			if(fj>0) //i and j are float
			{
				daux1 = 
					fi*D((int)i,(int)j)+
					(1-fi)*D((int)i+1,(int)j);
				daux2 =
					fi*D((int)i,(int)j+1)+
					(1-fi)*D((int)i+1,(int)j+1);
				return 
					fj*daux1 + 
					(1-fj)*daux2;

			}
			else // i is float
			{
				return
					fi*D((int)i,(int)j) +
					(1-fi)*D((int)i+1,(int)j);
			}
		else if(fj>0) // j is float
		{
			return 
				fj*D((int)i,(int)j) +
				(1-fj)*D((int)i,(int)j+1);
		}
		else // nonse is float
		{
			return D((int)i,(int)j);
		}
	}
	else
	{
		return 0;
	}
}

// function to acess the depth sensor data
float getDepth(int i, int j)
{
	if(i<depthw-1 && i >0 && j<depthh && j>0)
	{
		return D(i,j);
	}
	else
	{
		return 0;
	}
}

float getResult(int i, int j)
{
	if(i<depthw-1 && i >0 && j<depthh && j>0)
	{
		return result[i][j];
	}
	else
	{
		return 0;
	}
}

// funtction to apply sobel filtering
float applySobel(int dir, int x, int y)
{
	float retvalue = 0;
	if(dir == X_DIRECTION)
	{
		retvalue = 
			-1*getDepth(x-1,y-1)+
			1*getDepth(x+1,y-1)+
			-2*getDepth(x-1,y)+
			2*getDepth(x+1,y)+
			-1*getDepth(x-1,y+1)+
			1*getDepth(x+1,y+1);
	}
	else if(dir == Y_DIRECTION)
	{
		retvalue = 
			1*getDepth(x-1,y-1)+
			2*getDepth(x,y-1)+
			1*getDepth(x+1,y-1)+
			-1*getDepth(x-1,y+1)+
			-2*getDepth(x,y+1)+
			-1*getDepth(x+1,y+1);

	}
	return retvalue;
}

// funtction to apply sobel filtering with floating point
float applySobel(int dir, float x, float y)
{
	float retvalue = 0;
	if(dir == X_DIRECTION)
	{
		retvalue = 
			-1*getDepth(x-1,y-1)+
			1*getDepth(x+1,y-1)+
			-2*getDepth(x-1,y)+
			2*getDepth(x+1,y)+
			-1*getDepth(x-1,y+1)+
			1*getDepth(x+1,y+1);
	}
	else if(dir == Y_DIRECTION)
	{
		retvalue = 
			1*getDepth(x-1,y-1)+
			2*getDepth(x,y-1)+
			1*getDepth(x+1,y-1)+
			-1*getDepth(x-1,y+1)+
			-2*getDepth(x,y+1)+
			-1*getDepth(x+1,y+1);

	}
	return retvalue;
}

float applySobelResult(int dir, int x, int y)
{
	float retvalue = 0;
	if(dir == X_DIRECTION)
	{
		retvalue = 
			-1*getResult(x-1,y-1)+
			1*getResult(x+1,y-1)+
			-2*getResult(x-1,y)+
			2*getResult(x+1,y)+
			-1*getResult(x-1,y+1)+
			1*getResult(x+1,y+1);
	}
	else if(dir == Y_DIRECTION)
	{
		retvalue = 
			1*getResult(x-1,y-1)+
			2*getResult(x,y-1)+
			1*getResult(x+1,y-1)+
			-1*getResult(x-1,y+1)+
			-2*getResult(x,y+1)+
			-1*getResult(x+1,y+1);

	}
	return retvalue;
}

// normalize vector
void normalize(float * vector)
{
	float length = sqrt(
		(vector[X]*vector[X])+
		(vector[Y]*vector[Y])+
		(vector[Z]*vector[Z]));
	vector[X] = vector[X]/length;
	vector[Y] = vector[Y]/length;
	vector[Z] = vector[Z]/length;
}

// dot prodtuct
float dot(float * a, float * b)
{
	return a[X]*b[X] + a[Y]*b[Y] + a[Z]*b[Z];
}

// calculate diffuse light in point (only when not using openGL lighting)
float diffuseLight(float * normal, float * lightPosition, float * pointPosition)
{


	float lightDirection[3];
	for(int i = X ; i <= Z ; i++)
	{
		lightDirection[i] = lightPosition[i] - pointPosition[i];
	}
	normalize(lightDirection);
	return dot(lightDirection,normal);
}

// draw a vertex, calculating its normal
void drawVertex(int i, int j)
{
	openni::RGB888Pixel c = getColor(i,j);
	float d = getDepth(i,j);
	if(-d<MAX_DISTANCE && -d>0)
	{
		float normal[3];
		float point[3] = {(i-depthw/2),(depthh/2-j),d};



		normal[X] = applySobel(X_DIRECTION,i,j);
		normal[Y] = applySobel(Y_DIRECTION,i,j);
		normal[Z] = ZSTRENGTH;
		normalize(normal);

		//openni::RGB888Pixel c = getColor(i,j);
		//float c = diffuseLight(normal,lightPosition,point);
		//float c = 255;

		glColor3f(c.r,c.g,c.b);
		glNormal3f(
			normal[X],
			normal[Y],
			normal[Z]);
		glVertex4f(
			point[X],
			point[Y],
			point[Z],
			1.0f);
	}
}

void drawVertex_FromResult(int i, int j)
{
	float d = getResult(i,j);
	if(-d<MAX_DISTANCE && -d>0)
	{
		float normal[3];
		float point[3] = {(i-depthw/2),(depthh/2-j),d};



		normal[X] = applySobelResult(X_DIRECTION,i,j);
		normal[Y] = applySobelResult(Y_DIRECTION,i,j);
		normal[Z] = ZSTRENGTH;
		normalize(normal);

		//openni::RGB888Pixel c = getColor(i,j);
		//float c = diffuseLight(normal,lightPosition,point);
		//float c = 255;

		//glColor3f(c,c,c);
		glNormal3f(
			normal[X],
			normal[Y],
			normal[Z]);
		glVertex4f(
			point[X],
			point[Y],
			point[Z],
			1.0f);
	}
}


// draw a vertex, calculating its normal, interpolates if the number is floating
void drawVertex(float i, float j)
{
	float d = getDepth(i,j);
	if(-d<MAX_DISTANCE && -d>0)
	{
		float normal[3];
		float point[3] = {(i-depthw/2),(depthh/2-j),d};



		normal[X] = applySobel(X_DIRECTION,i,j);
		normal[Y] = applySobel(Y_DIRECTION,i,j);
		normal[Z] = ZSTRENGTH;
		normalize(normal);

		//openni::RGB888Pixel c = getColor(i,j);
		//float c = diffuseLight(normal,lightPosition,point);
		//float c = 255;

		//glColor3f(c,c,c);
		glNormal3f(
			normal[X],
			normal[Y],
			normal[Z]);
		glVertex4f(
			point[X],
			point[Y],
			point[Z],
			1.0f);
	}
}

// draw a vertex without calculating the normal
void drawVertexFast(int i, int j)
{
	float d = getDepth(i,j);
	if(-d<MAX_DISTANCE && -d>0)
	{
		
		float point[3] = {(i-depthw/2),(depthh/2-j),d};

		//openni::RGB888Pixel c = getColor(i,j);
		//float c = diffuseLight(normal,lightPosition,point);
		float c = 255;

		glColor3f(c,c,c);
		glVertex4f(
			point[X],
			point[Y],
			point[Z],
			1.0f);
	}
}

// draws a face, calculating the normal for each vertex
void drawFace(int i, int j, int offset=1)
{
	//FIRST TRIANGLE
	glBegin(GL_TRIANGLES);


	drawVertex(i,j);
	drawVertex(i+offset,j);
	drawVertex(i+offset,j+offset);
	glEnd();
	//SECOND TRIANGLE
	glBegin(GL_TRIANGLES);

	drawVertex(i+offset,j+offset);
	drawVertex(i,j+offset);
	drawVertex(i,j);
	glEnd();
}

// draws a face, calculating the normal for each vertex floating point
void drawFace(float i, float j, float offset=1)
{
	//FIRST TRIANGLE
	glBegin(GL_TRIANGLES);


	drawVertex(i,j);
	drawVertex(i+offset,j);
	drawVertex(i+offset,j+offset);
	glEnd();
	//SECOND TRIANGLE
	glBegin(GL_TRIANGLES);

	drawVertex(i+offset,j+offset);
	drawVertex(i,j+offset);
	drawVertex(i,j);
	glEnd();
}

// draws a face calculating only one normal
void drawFaceFast(int i, int j, int offset=1)
{
	float normal[3];

	normal[X] = applySobel(X_DIRECTION,i,j);
	normal[Y] = applySobel(Y_DIRECTION,i,j);
	normal[Z] = ZSTRENGTH;
	normalize(normal);
	//FIRST TRIANGLE
	glBegin(GL_TRIANGLES);
	glNormal3f(
		normal[X],
		normal[Y],
		normal[Z]);

	drawVertexFast(i,j);
	drawVertexFast(i+offset,j);
	drawVertexFast(i+offset,j+offset);
	glEnd();
	//SECOND TRIANGLE
	glBegin(GL_TRIANGLES);
	glNormal3f(
		normal[X],
		normal[Y],
		normal[Z]);
	drawVertexFast(i+offset,j+offset);
	drawVertexFast(i,j+offset);
	drawVertexFast(i,j);
	glEnd();
}
int step = 0;
#define MAX_STEP 60
#define KDEPTH 1000
void display()
{
	step++;

	// Clear Screen and Depth Buffer
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);		     
	glLoadIdentity();
	glLightfv(GL_LIGHT0,GL_POSITION,lightPosition);
	// Define a viewing transformation
	gluLookAt( 
		cameraX,cameraY,cameraZ, 
		0,0,-1000, 
		0,1,0);	

	lightPosition[X] = -(mouse[X]-320)*70;
	lightPosition[Y] = (mouse[Y]-240)*70;

	draw();
	//float cameraCalibrate;
	if(step>0)//step<MAX_STEP)
	{
		//printf("%d ",step);
		glBegin(GL_POINTS);
		for(int i=0 ; i<depthw-1 ; i+=OFFSET)
		{
			for(int j=0 ; j<depthh-1 ; j+=OFFSET)
			{
				
				//cameraCalibrate = -CVALUE/getDepth(i,j);
				/*
				int d = getDepth(i,j);
				for(int k = 0 ; k< KDEPTH ; k++)
				{
					sdf[i][j][k]+= k+d;
				}
				*/
				/*
				glColor3f(
					((float)getColor(i+cameraCalibrate,j).r)/255,
					((float)getColor(i+cameraCalibrate,j).g)/255,
					((float)getColor(i+cameraCalibrate,j).b)/255);
				*/
				drawVertex(i,j);
				//drawVertex(i,j);
			}
		
		}
		glEnd();
	}
	else if(step == MAX_STEP)
	{
		printf("%d step\n",MAX_STEP);
		//int minK = 0;
		//int minV = 100000;
		for(int i=0 ; i<depthw-1 ; i+=OFFSET)
		{
			for(int j=0 ; j<depthh-1 ; j+=OFFSET)
			{
				//minV = 10000000;
				result[i][j] = -1;
				for(int k=1 ; k<KDEPTH ; k++)
				{
					if(sdf[i][j][k]==0)
					{
						
						//minK = k;
						result[i][j] = k;
						break;
						//minV = sdf[i][j][k];
					}
				}
				//result[i][j] = minK;
				//printf("%d ",result[i][j]);
				
			}
		}
	}
	else
	{
		int r;
		printf("Begin printing...\n");
		glBegin(GL_POINTS);
		for(int i=0 ; i<depthw-1 ; i+=OFFSET)
		{
			for(int j=0 ; j<depthh-1 ; j+=OFFSET)
			{
				r = result[i][j];
				glColor3f(1,1,1);
				//printf("%d\n", result[i][j]);
				if(r>0)
				{
					glVertex3f((i-depthw/2),(depthh/2-j),-r);

				}
			}
		
		}
		glEnd();
	}

				  
 
	glutSwapBuffers();
}


void glutIdle()
{
	glutPostRedisplay();
}

void glutDisplay()
{
	display();
}

void glutKeyboard(unsigned char key, int x, int y)
{
	switch(key)
	{

	case 'a':
		cameraX-=DISTANCE;
		break;
	case 'd':
		cameraX+=DISTANCE;
		break;
	case 'w':
		cameraY+=DISTANCE;
		break;
	case 's':
		cameraY-=DISTANCE;
		break;
	case 'z':
		cameraZ+=DISTANCE;
		//CVALUE += 500;
		break;
	case 'x':
		//CVALUE -= 500;
		cameraZ-=DISTANCE;
		break;
	case 'f':
		offset_++;
		break;
	case 'g':
		if(offset_ > 1)
			offset_--;
		break;
	case 'c':
		maxDistance += 50;
		break;
	case 'v':
		maxDistance -=50;

	}
	int k = x+y;
	k = k+1;

}

void glutMousePassive(int x, int y)
{
	mouse[X] = x;
	mouse[Y] = y;
}

void initOpenGL(int argc, char **argv)
{
	
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
	glutInitWindowSize(640, 480);
	glutCreateWindow ("Kinect");

	glutSetCursor(GLUT_CURSOR_NONE);

	glutDisplayFunc(glutDisplay);
	glutIdleFunc(glutIdle);
	glutKeyboardFunc(glutKeyboard);
	glutPassiveMotionFunc(glutMousePassive);
	glDisable(GL_DEPTH_TEST);
	

    glMatrixMode(GL_PROJECTION);												
 
	// set the viewport
    glViewport(0, 0, 640,480);									
 
	// set matrix mode
    glMatrixMode(GL_PROJECTION);												
 
	// reset projection matrix
    glLoadIdentity();															
    //GLfloat aspect = (GLfloat) 640/480;
 
	// set up a perspective projection matrix
	
	float vfov = 45.0f;
	float aspect = 1;
	gluPerspective(vfov,aspect,0.5,20000);		
 
	// specify which matrix is the current matrix
	glMatrixMode(GL_MODELVIEW);													
    glShadeModel( GL_SMOOTH );
 
	// specify the clear value for the depth buffer
	glClearDepth( 1.0f );														
    glEnable( GL_DEPTH_TEST );
    glDepthFunc( GL_LEQUAL );

	glClearColor(0.0, 0.1, 0.0, 1.0);

	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);
	glEnable(GL_COLOR_MATERIAL);
	

	GLfloat lightDiffuse[] = {1.0, 1.0, 1.0}; 
	GLfloat lightSpecular[] = {1.0, 1.0, 1.0};
	GLfloat lightEmissive[] = {0.0, 0.0, 0.0};
	
	GLfloat diffuse[] = {0.0, 0.0, 0.0}; 
	GLfloat specular[] = {0.0, 0.0, 0.0};
	GLfloat emissive[] = {0.0, 0.0, 0.0};
	
	
	glLightfv(GL_LIGHT0, GL_SPECULAR, lightSpecular);
	glLightfv(GL_LIGHT0, GL_AMBIENT, lightEmissive);
	glLightfv(GL_LIGHT0, GL_DIFFUSE, lightDiffuse);
	
	glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, specular);
	glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, diffuse);
	glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, emissive);
	
	

}

int main(int argc, char** argv)
{

	depthw = dScreen.getWidth();
	depthh = dScreen.getHeight();
	
	screen = new openni::RGB888Pixel[depthw * depthh];
	fscreen = new int[depthw * depthh];

	for(int i=0 ; i<640 ; i++)
		for(int j=0 ; j<640 ;j++)
			for(int k=0 ; k<KDEPTH ; k++)
			{
				sdf[i][j][k] = 0;
			}
	for(int i=0 ; i<640 ; i++)
		for(int j=0 ; j<640 ;j++)
			result[i][j] = 0;

	for(unsigned int i=0 ; i<(unsigned int)depthw * depthh ; i++)
	{
		screen[i].r = 20;
		screen[i].g = 0;
		screen[i].b = 0;
		fscreen[i] = 0;
	}
	initOpenGL(argc,argv);
			printf("Camera left(a) right(d) \nup(w) down(s) \nforward(s) backward(z) \nSampling (f)less fine (g)more fine \nMax Distance increase(c) decrease(v)\n"); 
	glutMainLoop();

	return 0;
}

